#include "findNei.h"
#include "DetourCommon.h"
#include "DetourMath.h"

CFindNei::~CFindNei()
{
	Clear();
}

bool CFindNei::Init(const float cellSize)
{
	m_cellSize = cellSize;
	m_invCellSize = 1.0f / m_cellSize;
	// Allocate hashs buckets
	m_bucketsSize = dtNextPow2(hashSeed);
	return true;
}

void CFindNei::Clear()
{
	m_hashMap.clear();
	m_bounds[0] = 0xffff;
	m_bounds[1] = 0xffff;
	m_bounds[2] = -0xffff;
	m_bounds[3] = -0xffff;
}

void CFindNei::AddItem(const unsigned short id,
	const float minx, const float miny,
	const float maxx, const float maxy)
{
	const int iminx = (int)dtMathFloorf(minx * m_invCellSize);
	const int iminy = (int)dtMathFloorf(miny * m_invCellSize);
	const int imaxx = (int)dtMathFloorf(maxx * m_invCellSize);
	const int imaxy = (int)dtMathFloorf(maxy * m_invCellSize);

	m_bounds[0] = dtMin(m_bounds[0], iminx);
	m_bounds[1] = dtMin(m_bounds[1], iminy);
	m_bounds[2] = dtMax(m_bounds[2], imaxx);
	m_bounds[3] = dtMax(m_bounds[3], imaxy);

	for (int y = iminy; y <= imaxy; ++y)
	{
		for (int x = iminx; x <= imaxx; ++x)
		{
			int h = hashPos2(x, y, m_bucketsSize);
			Item item;
			item.x = (short)x;
			item.y = (short)y;
			item.id = id;
			auto ret = m_hashMap.insert(make_pair(h, item));
			
		}
	}

}

int CFindNei::QueryItems(const float minx, const float miny,
	const float maxx, const float maxy,
	unsigned short* ids, const int maxIds)
{
	const int iminx = (int)dtMathFloorf(minx * m_invCellSize);
	const int iminy = (int)dtMathFloorf(miny * m_invCellSize);
	const int imaxx = (int)dtMathFloorf(maxx * m_invCellSize);
	const int imaxy = (int)dtMathFloorf(maxy * m_invCellSize);

	int n = 0;

	for (int y = iminy; y <= imaxy; ++y)
	{
		for (int x = iminx; x <= imaxx; ++x)
		{
			const int h = hashPos2(x, y, m_bucketsSize);
			auto begin = m_hashMap.lower_bound(h);
			auto end = m_hashMap.upper_bound(h);
			while (begin != end)
			{
				Item item = begin->second;
				if ((int)item.x == x && (int)item.y == y)
				{
					// Check if the id exists already.
					const unsigned short* end = ids + n;
					unsigned short* i = ids;
					while (i != end && *i != item.id)
						++i;
					// Item not found, add it.
					if (i == end)
					{
						if (n >= maxIds)
							return n;
						ids[n++] = item.id;
					}
				}
				begin++;
			}
		}
	}
	return n;
}

int CFindNei::hashPos2(int x, int y, int n)
{
	return ((x * 73856093) ^ (y * 19349663)) & (n - 1);
}

unsigned int CFindNei::dtNextPow2(unsigned int v)
{
	v--;
	v |= v >> 1;
	v |= v >> 2;
	v |= v >> 4;
	v |= v >> 8;
	v |= v >> 16;
	v++;
	return v;
}

int CFindNei::GetItemCountAt(const int x, const int y)
{
	int n = 0;

	int h = hashPos2(x, y, m_bucketsSize);
	auto begin = m_hashMap.lower_bound(h);
	auto end = m_hashMap.upper_bound(h);
	while (begin != end)
	{
		Item item = begin->second;
		if ((int)item.x == x && (int)item.y == y)
		{
			++n;
		}
	}

	return n;
}
